// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RobotiqSModelTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.Robotiq
{
    using System;
    using Microsoft.Robotics.Hardware.Robotiq;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Hardware.Modbus;

    /// <summary>
    /// Tests for the Robotiq S Model gripper that do not require hardware
    /// </summary>
    [TestClass]
    public class RobotiqSModelTests
    {
        /// <summary>
        /// Test building a basic command
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void ConstructRobotiqSModelBasicCommandTest()
        {
            bool activation = true;
            RobotiqSModelGripModes gripMode = RobotiqSModelGripModes.Pinch;
            bool goToPosition = true;
            byte position = 127;
            byte speed = 128;
            byte force = 129;

            RobotiqSModelBasicCommand command = new RobotiqSModelBasicCommand(
                activation,
                gripMode,
                goToPosition,
                position,
                speed,
                force);

            Assert.AreEqual(activation, command.Activate);
            Assert.AreEqual(gripMode, command.GripMode);
            Assert.AreEqual(goToPosition, command.GoToPosition);
            Assert.AreEqual(position, command.Position);
            Assert.AreEqual(speed, command.Speed);
            Assert.AreEqual(force, command.Force);
        }

        /// <summary>
        /// Test setting a basic command
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void SetRobotiqSModelBasicCommandTest()
        {
            // use example of opening gripper at full speed and full force on pg. 73 of Robotiq manual
            MockSimpleModbusTCPConnection mockModbusConnection = new MockSimpleModbusTCPConnection();

            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbusConnection);

            mockModbusConnection.ExpectedRegisterValues = new ushort[] { 0x0009, 0x0000, 0xFFFF };

            gripper.SetBasicCommand(new RobotiqSModelBasicCommand(
                true,
                RobotiqSModelGripModes.Basic,
                true,
                0,
                255,
                255));
        }

        /// <summary>
        /// Test getting the finger status
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void GetFingerStatusTest()
        {
            MockSimpleModbusTCPConnection mockModbusConnection = new MockSimpleModbusTCPConnection();

            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbusConnection);

            // in motion 
            Assert.AreEqual(
                RobotiqSModelFingerStatus.InMotion,
                RobotiqSModelGripper.GetFingerStatus(RobotiqSModelValues.FingerInMotion));

            Assert.AreEqual(
                RobotiqSModelFingerStatus.AtRequestedPosition,
                RobotiqSModelGripper.GetFingerStatus(RobotiqSModelValues.FingerIsAtRequestedPosition));

            Assert.AreEqual(
                RobotiqSModelFingerStatus.StoppedDueToContactWhileClosing,
                RobotiqSModelGripper.GetFingerStatus(RobotiqSModelValues.FingerHasStoppedDueToContactWhileClosing));

            Assert.AreEqual(
                RobotiqSModelFingerStatus.StoppedDueToContactWhileOpening,
                RobotiqSModelGripper.GetFingerStatus(RobotiqSModelValues.FingerHasStoppedDueToContactWhileOpening));
        }

        /// <summary>
        /// Test getting the grip mode
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void GetGripModeTest()
        {
            MockSimpleModbusTCPConnection mockModbusConnection = new MockSimpleModbusTCPConnection();

            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbusConnection);

            Assert.AreEqual(
                RobotiqSModelGripModes.Basic,
                RobotiqSModelGripper.GetGripMode(RobotiqSModelValues.BasicMode));

            Assert.AreEqual(
                RobotiqSModelGripModes.Pinch,
                RobotiqSModelGripper.GetGripMode(RobotiqSModelValues.PinchMode));

            Assert.AreEqual(
                RobotiqSModelGripModes.Scissor,
                RobotiqSModelGripper.GetGripMode(RobotiqSModelValues.ScissorMode));

            Assert.AreEqual(
                RobotiqSModelGripModes.Wide,
                RobotiqSModelGripper.GetGripMode(RobotiqSModelValues.WideMode));
        }

        /// <summary>
        /// Test getting the gripper status
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void GetGripperStatusTest()
        {
            MockSimpleModbusTCPConnection mockModbusConnection = new MockSimpleModbusTCPConnection();

            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbusConnection);

            Assert.AreEqual(
                RobotiqSModelStatus.ActivationAndModeChangeComplete,
                RobotiqSModelGripper.GetGripperStatus(RobotiqSModelValues.ActivationAndModeChangeComplete));

            Assert.AreEqual(
                RobotiqSModelStatus.ActivationInProgress,
                RobotiqSModelGripper.GetGripperStatus(RobotiqSModelValues.ActivationInProgress));

            Assert.AreEqual(
                RobotiqSModelStatus.ModeChangeIsInProgress,
                RobotiqSModelGripper.GetGripperStatus(RobotiqSModelValues.ModeChangeInProgress));

            Assert.AreEqual(
                RobotiqSModelStatus.Reset,
                RobotiqSModelGripper.GetGripperStatus(RobotiqSModelValues.Reset));
        }

        /// <summary>
        /// Test getting the gripper status
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void GetGripStatusTest()
        {
            MockSimpleModbusTCPConnection mockModbusConnection = new MockSimpleModbusTCPConnection();

            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbusConnection);

            Assert.AreEqual(
                RobotiqSModelGripStatus.AllFingersStoppedBeforeRequestedPosition,
                RobotiqSModelGripper.GetGripStatus(RobotiqSModelValues.AllFingersStoppedBeforeReference));

            Assert.AreEqual(
                RobotiqSModelGripStatus.InMotion,
                RobotiqSModelGripper.GetGripStatus(RobotiqSModelValues.GripperInMotion));

            Assert.AreEqual(
                RobotiqSModelGripStatus.OneOrTwoFingersStoppedBeforeRequestedPosition,
                RobotiqSModelGripper.GetGripStatus(RobotiqSModelValues.OneOrTwoFingersStoppedBeforeReference));

            Assert.AreEqual(
                RobotiqSModelGripStatus.Stopped,
                RobotiqSModelGripper.GetGripStatus(RobotiqSModelValues.AllFingersReachedReference));
        }

        /// <summary>
        /// Test <c>ModbusRegistersToRobotiquRegisters</c>.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void ModbusRegistersToRobotiqRegistersTest()
        {
            ushort[] testModbusRegisters = 
            {
                0x0,
                0xFFFF,
                0x00FF,
                0xFF00
            };

            byte[] expectedResult =
            {
                0x0,
                0x0,
                0xFF,
                0xFF,
                0xFF,
                0x0,
                0x0,
                0xFF
            };

            byte[] result = RobotiqSModelGripper.ModbusRegistersToRobotiqRegisters(testModbusRegisters);

            Assert.AreEqual(expectedResult.Length, result.Length, "Got expected number of registers in result");

            for (int i = 0; i < result.Length; ++i)
            {
                Assert.AreEqual(expectedResult[i], result[i], "Expected result at position " + i);
            }
        }

        /// <summary>
        /// Test <c>RobotiqSModelGripper.UpdateState()</c> with zero for all register values.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void RobotiqSModelGripperUpdateStateAllZerosTest()
        {
            MockSimpleModbusTCPConnection mockModbus = new MockSimpleModbusTCPConnection();
            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbus);

            ushort[] registers = 
            {
                0x0000,
                0x0000,
                0x0000,
                0x0000,
                0x0000,
                0x0000,
                0x0000,
                0x0000
            };

            mockModbus.RegisterValuesToReturn = registers;

            gripper.UpdateState();

            Assert.IsFalse(gripper.State.IsActivated, "Gripper activation state is correct");
            Assert.AreEqual(RobotiqSModelGripModes.Basic, gripper.State.GripMode, "Gripper mode is correct");
            Assert.IsFalse(gripper.State.GoToPosition, "Go to position state is correct");
            Assert.AreEqual(RobotiqSModelStatus.Reset, gripper.State.DeviceStatus, "Gripper status is correct");
            Assert.AreEqual(RobotiqSModelGripStatus.InMotion, gripper.State.GripStatus, "Grip status is correct");
            Assert.AreEqual(RobotiqSModelFingerStatus.InMotion, gripper.State.FingerAState.FingerStatus, "Finger A state is correct");
            Assert.AreEqual(RobotiqSModelFingerStatus.InMotion, gripper.State.FingerBState.FingerStatus, "Finger B state is correct");
            Assert.AreEqual(RobotiqSModelFingerStatus.InMotion, gripper.State.FingerCState.FingerStatus, "Finger C state is correct");
            Assert.AreEqual(RobotiqSModelFingerStatus.InMotion, gripper.State.ScissorState.FingerStatus, "Scissor state is correct");
            Assert.AreEqual(RobotiqSModelFaults.NoFault, gripper.State.Fault, "Fault status is correct");
            Assert.AreEqual(0, gripper.State.FingerAState.CommandedPosition, "Finger A commanded position is correct");
            Assert.AreEqual(0, gripper.State.FingerBState.CommandedPosition, "Finger B commanded position is correct");
            Assert.AreEqual(0, gripper.State.FingerCState.CommandedPosition, "Finger C commanded position is correct");
            Assert.AreEqual(0, gripper.State.ScissorState.CommandedPosition, "Scissor commanded position is correct");
            Assert.AreEqual(0, gripper.State.FingerAState.Position, "Finger A position is correct");
            Assert.AreEqual(0, gripper.State.FingerBState.Position, "Finger B position is correct");
            Assert.AreEqual(0, gripper.State.FingerCState.Position, "Finger C position is correct");
            Assert.AreEqual(0, gripper.State.ScissorState.Position, "Scissor position is correct");
            Assert.AreEqual(0, gripper.State.FingerAState.CurrentInMilliAmps, "Finger A current in mA is correct");
            Assert.AreEqual(0, gripper.State.FingerBState.CurrentInMilliAmps, "Finger B current in mA is correct");
            Assert.AreEqual(0, gripper.State.FingerCState.CurrentInMilliAmps, "Finger C current in mA is correct");
            Assert.AreEqual(0, gripper.State.ScissorState.CurrentInMilliAmps, "Scissor current in mA is correct");
        }

        /// <summary>
        /// Test <c>RobotiqSModelGripper.UpdateState()</c> with non zero register values.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void RobotiqSModelGripperUpdateStateNonZeroTest()
        {
            MockSimpleModbusTCPConnection mockModbus = new MockSimpleModbusTCPConnection();
            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbus);

            byte gripperActivated = RobotiqSModelValues.GripperActivated;
            byte gripMode = RobotiqSModelValues.WideMode;
            byte goToPositionActive = RobotiqSModelValues.GoToPositionActive;
            byte gripperStatus = RobotiqSModelValues.ModeChangeInProgress;
            byte gripStatus = RobotiqSModelValues.OneOrTwoFingersStoppedBeforeReference;

            byte fingerAMotionStatus = RobotiqSModelValues.FingerHasStoppedDueToContactWhileClosing;
            byte fingerBMotionStatus = RobotiqSModelValues.FingerHasStoppedDueToContactWhileOpening;
            byte fingerCMotionStatus = RobotiqSModelValues.FingerIsAtRequestedPosition;
            byte scissorMotionStatus = RobotiqSModelValues.FingerInMotion;

            byte faultStatus = (byte)RobotiqSModelFaults.CommunicationChipNotReady;

            byte commandedPosition = 128;

            byte fingerAActualPosition = 1;
            byte fingerBActualPosition = 20;
            byte fingerCActualPosition = 50;
            byte scissorActualPosition = 255;

            byte fingerACurrent = 5;
            byte fingerBCurrent = 125;
            byte fingerCCurrent = 200;
            byte scissorCurrent = 7;

            ushort[] registers = this.BuildRegisters(
                gripperActivated,
                gripMode,
                goToPositionActive,
                gripperStatus,
                gripStatus,
                fingerAMotionStatus,
                fingerBMotionStatus,
                fingerCMotionStatus,
                scissorMotionStatus,
                faultStatus,
                commandedPosition,
                fingerAActualPosition,
                fingerBActualPosition,
                fingerCActualPosition,
                scissorActualPosition,
                fingerACurrent,
                fingerBCurrent,
                fingerCCurrent,
                scissorCurrent);

            mockModbus.RegisterValuesToReturn = registers;

            gripper.UpdateState();

            Assert.AreEqual(gripperActivated == RobotiqSModelValues.GripperActivated, gripper.State.IsActivated, "Gripper activation state is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetGripMode(gripMode), gripper.State.GripMode, "Gripper mode is correct");
            Assert.AreEqual(goToPositionActive == RobotiqSModelValues.GoToPositionActive, gripper.State.GoToPosition, "Go to position state is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetGripperStatus(gripperStatus), gripper.State.DeviceStatus, "Gripper status is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetGripStatus(gripStatus), gripper.State.GripStatus, "Grip status is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetFingerStatus(fingerAMotionStatus), gripper.State.FingerAState.FingerStatus, "Finger A state is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetFingerStatus(fingerBMotionStatus), gripper.State.FingerBState.FingerStatus, "Finger B state is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetFingerStatus(fingerCMotionStatus), gripper.State.FingerCState.FingerStatus, "Finger C state is correct");
            Assert.AreEqual(RobotiqSModelGripper.GetFingerStatus(scissorMotionStatus), gripper.State.ScissorState.FingerStatus, "Scissor state is correct");
            Assert.AreEqual(faultStatus, (byte)gripper.State.Fault, "Fault status is correct");
            Assert.AreEqual(commandedPosition, gripper.State.FingerAState.CommandedPosition, "Finger A commanded position is correct");
            Assert.AreEqual(commandedPosition, gripper.State.FingerBState.CommandedPosition, "Finger B commanded position is correct");
            Assert.AreEqual(commandedPosition, gripper.State.FingerCState.CommandedPosition, "Finger C commanded position is correct");
            Assert.AreEqual(commandedPosition, gripper.State.ScissorState.CommandedPosition, "Scissor commanded position is correct");
            Assert.AreEqual(fingerAActualPosition, gripper.State.FingerAState.Position, "Finger A position is correct");
            Assert.AreEqual(fingerBActualPosition, gripper.State.FingerBState.Position, "Finger B position is correct");
            Assert.AreEqual(fingerCActualPosition, gripper.State.FingerCState.Position, "Finger C position is correct");
            Assert.AreEqual(scissorActualPosition, gripper.State.ScissorState.Position, "Scissor position is correct");
            Assert.AreEqual(fingerACurrent * 10.0, gripper.State.FingerAState.CurrentInMilliAmps, "Finger A current in mA is correct");
            Assert.AreEqual(fingerBCurrent * 10.0, gripper.State.FingerBState.CurrentInMilliAmps, "Finger B current in mA is correct");
            Assert.AreEqual(fingerCCurrent * 10.0, gripper.State.FingerCState.CurrentInMilliAmps, "Finger C current in mA is correct");
            Assert.AreEqual(scissorCurrent * 10.0, gripper.State.ScissorState.CurrentInMilliAmps, "Scissor current in mA is correct");
        }

        /// <summary>
        /// Test <c>RobotiqSModelGripper.ResetGripper()</c>.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [Priority(0)]
        public void RobotiqSModelGripperResetTest()
        {
            MockSimpleModbusTCPConnection mockModbus = new MockSimpleModbusTCPConnection();
            RobotiqSModelGripper gripper = new RobotiqSModelGripper(mockModbus);

            byte gripperActivated = 0;
            byte gripMode = RobotiqSModelValues.WideMode;
            byte goToPositionActive = RobotiqSModelValues.GoToPositionActive;
            byte gripperStatus = RobotiqSModelValues.ActivationAndModeChangeComplete;
            byte gripStatus = RobotiqSModelValues.AllFingersReachedReference;

            byte fingerAMotionStatus = RobotiqSModelValues.FingerIsAtRequestedPosition;
            byte fingerBMotionStatus = RobotiqSModelValues.FingerIsAtRequestedPosition;
            byte fingerCMotionStatus = RobotiqSModelValues.FingerIsAtRequestedPosition;
            byte scissorMotionStatus = RobotiqSModelValues.FingerIsAtRequestedPosition;

            byte faultStatus = (byte)RobotiqSModelFaults.CommunicationChipNotReady;

            byte commandedPosition = 0;

            byte fingerAActualPosition = 0;
            byte fingerBActualPosition = 0;
            byte fingerCActualPosition = 0;
            byte scissorActualPosition = 0;

            byte fingerACurrent = 0;
            byte fingerBCurrent = 0;
            byte fingerCCurrent = 0;
            byte scissorCurrent = 0;

            ushort[] registers = this.BuildRegisters(
                gripperActivated,
                gripMode,
                goToPositionActive,
                gripperStatus,
                gripStatus,
                fingerAMotionStatus,
                fingerBMotionStatus,
                fingerCMotionStatus,
                scissorMotionStatus,
                faultStatus,
                commandedPosition,
                fingerAActualPosition,
                fingerBActualPosition,
                fingerCActualPosition,
                scissorActualPosition,
                fingerACurrent,
                fingerBCurrent,
                fingerCCurrent,
                scissorCurrent);

            // register values that are expected to be sent over modbus
            // in response to call to ResetGripper()
            ushort[] expectedRegisterVaules =
            {
                0x0001,
                0x0000,
                0x0000
            };

            mockModbus.RegisterValuesToReturn = registers;
            mockModbus.ExpectedRegisterValues = expectedRegisterVaules;

            gripper.ResetGripper(true, 1000);
        }

        /// <summary>
        /// Builds (ModbusTCP) given a set of values for state of Robotiq S Model
        /// </summary>
        /// <param name="gripperActivated">Gripper activation status.</param>
        /// <param name="gripMode">Grip mode.</param>
        /// <param name="goToPositionActive">Is go to position active.</param>
        /// <param name="gripperStatus">Gripper status.</param>
        /// <param name="gripStatus">Grip status.</param>
        /// <param name="fingerAMotionStatus">Finger A motion status.</param>
        /// <param name="fingerBMotionStatus">Finger B motion status.</param>
        /// <param name="fingerCMotionStatus">Finger C motion status.</param>
        /// <param name="scissorMotionStatus">Scissor motion status.</param>
        /// <param name="faultStatus">Fault status.</param>
        /// <param name="commandedPosition">Commanded position (same for all fingers).</param>
        /// <param name="fingerAActualPosition">Actual position of finger A.</param>
        /// <param name="fingerBActualPosition">Actual position of finger B.</param>
        /// <param name="fingerCActualPosition">Actual position of finger C.</param>
        /// <param name="scissorActualPosition">Actual position of scissor.</param>
        /// <param name="fingerACurrent">Current in finger A.</param>
        /// <param name="fingerBCurrent">Current in finger B.</param>
        /// <param name="fingerCCurrent">Current in finger C.</param>
        /// <param name="scissorCurrent">Current in scissor.</param>
        /// <returns>ModbusTCP registers corresponding to given state of Robotiq S Model</returns>
        private ushort[] BuildRegisters(
            byte gripperActivated,
            byte gripMode,
            byte goToPositionActive,
            byte gripperStatus,
            byte gripStatus,
            byte fingerAMotionStatus,
            byte fingerBMotionStatus,
            byte fingerCMotionStatus,
            byte scissorMotionStatus,
            byte faultStatus,
            byte commandedPosition,
            byte fingerAActualPosition,
            byte fingerBActualPosition,
            byte fingerCActualPosition,
            byte scissorActualPosition,
            byte fingerACurrent,
            byte fingerBCurrent,
            byte fingerCCurrent,
            byte scissorCurrent)
        {
            byte statusByte = (byte)(gripperActivated | gripMode | goToPositionActive | gripperStatus | gripStatus);

            byte fingerMotionStatusByte = (byte)(fingerAMotionStatus | (fingerBMotionStatus << 2) | (fingerCMotionStatus << 4) | (scissorMotionStatus << 6));

            ushort[] registers = 
            {
                (ushort)(statusByte | (fingerMotionStatusByte << 8)),
                (ushort)(faultStatus | (commandedPosition << 8)),
                (ushort)(fingerAActualPosition | (fingerACurrent << 8)),
                (ushort)(fingerBActualPosition << 8),
                (ushort)(fingerBCurrent),
                (ushort)(fingerCActualPosition | (fingerCCurrent << 8)),
                (ushort)(scissorActualPosition << 8),
                (ushort)(scissorCurrent)
            };

            return registers;
        }
    }
}
